// This sketch interfaces with the Pololu servo controller at:
// http://www.pololu.com/catalog/product/207
// All transmitter commands (1000-2000) are converted to servo commands (0-254)

//Debug data
#define DEBUG 0
#define BAUD 9600

// Define pin assignments
#define LEDPIN 13
#define THROTTLEPIN 2
#define ROLLPIN 3
#define PITCHPIN 4
#define YAWPIN 5

// Define servo assignments
#define FRONTCHANNEL 0
#define RIGHTCHANNEL 1
#define REARCHANNEL 2
#define LEFTCHANNEL 3

// Define transmitter commands
#define THROTTLETIMEOUT 25000
#define PULSETIMEOUT 20000
int throttle = 0;
int roll = 0;
int rollLeft = 0;
int rollRight = 0;
int pitch = 0;
int pitchUp = 0;
int pitchDown = 0;
int yaw = 0;
int yawLeft = 0;
int yawRight = 0;

// Define motor commands
byte frontCommand = 0;
byte rightCommand = 0;
byte rearCommand = 0;
byte leftCommand = 0;

void setup() {
  Serial.begin(BAUD);
  analogReference(EXTERNAL); // Current external ref is 3.3V
  pinMode(LEDPIN, OUTPUT);
  digitalWrite(LEDPIN, HIGH);
  pinMode(THROTTLEPIN, INPUT);
  pinMode(ROLLPIN, INPUT);
  pinMode(PITCHPIN, INPUT);
  pinMode(YAWPIN, INPUT);
  digitalWrite(LEDPIN, LOW);
  
  //Provide time for ESC's to initialize
  motorCommand(FRONTCHANNEL, frontCommand);
  motorCommand(RIGHTCHANNEL, rightCommand);
  motorCommand(REARCHANNEL, rearCommand);
  motorCommand(LEFTCHANNEL, leftCommand);
  digitalWrite(LEDPIN, HIGH);
  delay(5000);
  digitalWrite(LEDPIN, LOW);
}

void loop() {
  // Read transmitter commands
  throttle = constrain(int(pulseIn(THROTTLEPIN, HIGH, THROTTLETIMEOUT) - 1000), 0, 1000) *0.254; 
  
  roll = constrain(int(pulseIn(ROLLPIN, HIGH, PULSETIMEOUT) - 1000), 0, 1000) * 0.254;
  if (roll <= 127) {
    rollRight = 127 - roll;
    rollLeft = 0;
  }
  else {
    rollRight = 0;
    rollLeft = roll - 127;
  }
  
  pitch = constrain(int(pulseIn(PITCHPIN, HIGH, PULSETIMEOUT) - 1000), 0, 1000) * 0.254;
  if (pitch <=127) {
    pitchDown = 0;
    pitchUp = 127 - pitch;
  }
  else {
    pitchDown = pitch - 127;
    pitchUp = 0;
  }
  
  yaw = constrain(int(pulseIn(YAWPIN, HIGH, PULSETIMEOUT) - 1000), 0, 1000) *0.254;
  yawRight = 127 - yaw;
  yawLeft = yaw - 127;

  // Calculate motor commands
  frontCommand = constrain(throttle + pitchUp + yawLeft, 0, 254); 
  rightCommand = constrain(throttle + rollLeft + yawRight, 0, 254);
  rearCommand = constrain(throttle + pitchDown + yawLeft, 0, 254);
  leftCommand = constrain(throttle + rollRight + yawRight, 0, 254);

  // Command motors
  if (DEBUG == 0) {
    motorCommand(FRONTCHANNEL, frontCommand);
    motorCommand(RIGHTCHANNEL, rightCommand);
    motorCommand(REARCHANNEL, rearCommand);
    motorCommand(LEFTCHANNEL, leftCommand);
  }

  if (DEBUG == 1) xmitData();
}

void motorCommand(byte channel, byte command){  
  Serial.print(255, BYTE);
  Serial.print(channel, BYTE);
  Serial.print(command, BYTE);
}

void xmitData(){
  digitalWrite(LEDPIN, HIGH);
  Serial.print(throttle);
  Serial.print(",");
  Serial.print(roll);
  Serial.print(",");
  Serial.print(pitch);
  Serial.print(",");
  Serial.print(yaw);
  Serial.print(",");
  Serial.print(frontCommand, DEC);
  Serial.print(",");
  Serial.print(leftCommand, DEC);
  Serial.print(",");
  Serial.print(rightCommand, DEC);
  Serial.print(",");
  Serial.println(rearCommand, DEC);
  digitalWrite(LEDPIN, LOW);  
}
